#include "KinFuPostProcess.h"
#include <boost\thread\thread.hpp>
#include <iostream>


KinFuPostProcess::KinFuPostProcess() : 
width(640), height(480), 
init(false), terminate(false), 
pos(), h(width * height), seg(width, height){
	v_cloud_ptr_= pcl::PointCloud<pcl::gpu::KinfuTracker::PointType>::Ptr 
		(new pcl::PointCloud<pcl::gpu::KinfuTracker::PointType>);
	
	
	n_cloud_ptr_ = pcl::PointCloud<pcl::gpu::KinfuTracker::NormalType>::Ptr 
		(new pcl::PointCloud<pcl::gpu::KinfuTracker::NormalType>);
	
	//Asynchronous launch
	boost::thread proc(
		boost::bind(&KinFuPostProcess::runAsynchronousProcedures, this)
	);
	
}

KinFuPostProcess::~KinFuPostProcess(){
	terminate = true;
	v_cloud_ptr_->~PointCloud();
	n_cloud_ptr_->~PointCloud();
	delete[] pos.data;
	h.~Frame();	
}


void KinFuPostProcess::runAsynchronousProcedures(){
	
	//wait for KinFu initialization and sufficient amount of reconstruction data
	boost::this_thread::sleep(boost::posix_time::millisec(5000));
	
	do{		
		h.segmentPlanarStructures(&seg);
		h.integratePlanarStructures();

		//sleep
		boost::this_thread::sleep(boost::posix_time::millisec(500));

	} while(!terminate);
}


void KinFuPostProcess::process(pcl::gpu::KinfuTracker* kinfu){
	kinfu_localReference = kinfu;	
	
	if(!init){
		height = kinfu_localReference->rows();
		width = kinfu_localReference->cols();
		init = true;
	}
	
	extractVertices();
	extractNormals();
	extractCameraPose();	
}



void KinFuPostProcess::extractVertices(){
	int c;
	pcl::gpu::DeviceArray2D<pcl::gpu::KinfuTracker::PointType> vertices;

	kinfu_localReference->getLastFrameCloud(vertices);
	vertices.download(v_cloud_ptr_->points, c);
	int numCloud = v_cloud_ptr_->size();

	float3* temp = new float3[width * height];
	for(int i = 0; i < numCloud; i++){
		pcl::PointXYZ* p = &v_cloud_ptr_->at(i);
		temp[i].x = p->x;
		temp[i].y = p->y;
		temp[i].z = p->z;
	}

	//verticeData.insertDataFromHost(temp);
	h.setVertices(temp);
	delete[] temp;
}


void KinFuPostProcess::extractNormals(){
	int c;
	pcl::gpu::DeviceArray2D<pcl::gpu::KinfuTracker::NormalType> normals;

	kinfu_localReference->getLastFrameNormals(normals);
	normals.download(n_cloud_ptr_->points, c);
	int numNormal = n_cloud_ptr_->size();

	float3* temp = new float3[width * height];
	for(int i = 0; i < numNormal; i++){
		pcl::gpu::KinfuTracker::NormalType* n = &n_cloud_ptr_->at(i);
		temp[i].x = n->normal_x;
		temp[i].y = n->normal_y;
		temp[i].z = n->normal_z;
	}

	//normalData.insertDataFromHost(temp);
	h.setNormal(temp);
	delete[] temp;	

}

void KinFuPostProcess::extractCameraPose(){
	Eigen::Affine3f pose = kinfu_localReference->getCameraPose();
	for(int i = 0; i < 16; i++)
		pos.data[i] = pose.data()[i];

	h.setCameraPos(&pos);	
}


void KinFuPostProcess::printCameraPose(){
	std::cout << "Printing current camera position" << std::endl;
	std::cout << pos.data[0] << "\t" << pos.data[4] << "\t" << pos.data[8] << "\t" << pos.data[12] << std::endl;
	std::cout << pos.data[1] << "\t" << pos.data[5] << "\t" << pos.data[9] << "\t" << pos.data[13] << std::endl;
	std::cout << pos.data[2] << "\t" << pos.data[6] << "\t" << pos.data[10] << "\t" << pos.data[14] << std::endl;
	std::cout << pos.data[3] << "\t" << pos.data[7] << "\t" << pos.data[11] << "\t" << pos.data[15] << std::endl;
	std::cout << std::endl;
}
